#ifndef UTILS_H_
#define UTILS_H_

#include <stdlib.h>
#include <assert.h>

#include <omp.h>

template<typename T>
void swap(T &a, T &b) {
    T c = a;
    a = b;
    b = c;
}

namespace Utils {

    typedef struct {
        int key, value;
    } Item;

    /*
     *        0
     *       /  \
     *      1    2
     *     / \  / \
     *    3  4  5  6
     *
     *     x    floor[(x-1)/2]
     *   /   \       |
     * 2x+1 2x+2     x
     *
     */
    class Heap {

    private:
        Item *data;
        int *index;
        int len;

        inline int fa(int x) {
            return (x - 1) / 2;
        }

        void up(int x) {
            while (x > 0) {
                int tmp = (x - 1) / 2;
                if (data[tmp].value > data[x].value) {
                    swap(data[tmp], data[x]);
                    index[data[tmp].key] = tmp;
                    index[data[x].key] = x;
                    x = tmp;
                } else
                    break;
            }
        }

        void down(int x) {
            while (x * 2 + 1 < len) {
                int tmp = (x * 2 + 2 >= len || data[x * 2 + 1].value < data[x * 2 + 2].value) ? x * 2 + 1 : x * 2 + 2;
                if (data[tmp].value < data[x].value) {
                    swap(data[tmp], data[x]);
                    index[data[tmp].key] = tmp;
                    index[data[x].key] = x;
                    x = tmp;
                } else
                    break;
            }
        }

    public:
        Heap(int capacity) :
                len(0) {
            data = (Item*) malloc(sizeof(Item) * capacity);
            assert(data != NULL);
            index = (int*) malloc(sizeof(int) * capacity);
            assert(index !=NULL);
            memset(index, 0xff, sizeof(int) * capacity);
        }

        void add(int key, int value) {
            data[len].key = key;
            data[len].value = value;
            index[key] = len;
            ++len;
            up(len - 1);
        }

        void update(int key, int value) {
            data[index[key]].value = value;
            up(index[key]);
        }

        void addOrUpdate(int key, int value) {
            if (index[key] == -1)
                add(key, value);
            else if (value < data[index[key]].value) update(key, value);
        }

        void removeTop(Item *pData) {
            --len;
            swap(data[0], data[len]);
            if (pData != NULL) *pData = data[len];
            index[data[0].key] = 0;
            index[data[len].key] = -1;
            down(0);
        }

        const Item& top() {
            return data[0];
        }

        bool empty() const {
            return len == 0;
        }

        ~Heap() {
            free(data);
            free(index);
        }

        void print() {
            for (int i = 0; i < len; ++i)
                printf("(%d, %d) ", data[i].key, data[i].value);
            printf("\n");
        }
    };

    class Stack {
    protected:
        Item *data;
        int *index;
        int len;

    public:
        Stack(int capacity) :
                len(0) {
            data = (Item*) malloc(sizeof(Item) * capacity);
            assert(data != NULL);
            index = (int *) malloc(sizeof(int) * capacity);
            assert(index != NULL);
            memset(index, 0xff, sizeof(int) * capacity);
        }

        inline void addOrUpdate(int key, int value) {
            if (index[key] != -1) {
                if (data[index[key]].value > value) data[index[key]].value = value;
            } else {
                data[len].key = key;
                data[len].value = value;
                index[key] = len;
                ++len;
            }
        }

        bool empty() const {
            return len == 0;
        }

        inline void removeTop(Item *pTop) {
            assert(len != 0);
            --len;
            index[data[len].key] = -1;
            if (pTop != NULL) *pTop = data[len];
        }

        const Item& top() {
            assert(len > 0);
            return data[len - 1];
        }

        virtual ~Stack() {
            free(data);
            free(index);
        }
    };

    class OMPLockStack: public Stack {

        // lock here
        omp_lock_t lock;
    public:
        OMPLockStack(int capacity) :
                Stack(capacity) {
            omp_init_lock(&lock);
        }

        /**
         * Insert if Not Exists or Try to Update if Exists
         * Thread safe.
         */
        void addOrUpdateSafe(int key, int value) {
            omp_set_lock(&lock);
            addOrUpdate(key, value);
            omp_unset_lock(&lock);
        }

        void removeTopSafe(Item *pTop) {
            omp_set_lock(&lock);
            removeTop(pTop);
            omp_unset_lock(&lock);
        }

        const Item& top() {
            assert(len > 0);
            return data[len - 1];
        }

        ~OMPLockStack() {
            omp_destroy_lock(&lock);
        }
    };

    /*
     * Single Producer and Single Consumer
     */
    template<int capacity>
    class SmallConcurrentQueue {

        static const int MASK;
        volatile int head, tail;

        Item data[capacity];

    public:
        SmallConcurrentQueue() :
                head(0), tail(0) {
        }

        inline bool push(int key, int value) {
            if (((tail + 1) & MASK) == head) return false;
            data[tail].key = key;
            data[tail].value = value;
            tail = (tail + 1) & MASK;
            return true;
        }

        inline bool push(Item *item) {
            if (((tail + 1) & MASK) == head) return false;
            data[tail] = *item;
            tail = (tail + 1) & MASK;
            return true;
        }

        inline bool pop(Item *item) {
            if (head == tail) return false;
            if (item != NULL) *item = data[head];
            head = (head + 1) & MASK;
            return true;
        }

        inline bool pop(Item &item) {
            if (head == tail) return false;
            item = data[head];
            head = (head + 1) & MASK;
            return true;
        }

        inline bool empty() const {
            return head == tail;
        }

        ~SmallConcurrentQueue() {
        }
    };
    template<int capacity>
    const int SmallConcurrentQueue<capacity>::MASK = capacity - 1;

    template<int CAPACITY>
    class TwoDConcurrentQueue {
        SmallConcurrentQueue<CAPACITY> *queues;
        int thread_nums;
    public:
        TwoDConcurrentQueue(int thread_nums) :
                thread_nums(thread_nums) {
            queues = new SmallConcurrentQueue<CAPACITY> [thread_nums * thread_nums];
        }
        inline SmallConcurrentQueue<CAPACITY>* get(int row, int col) const {
            return queues + row * thread_nums + col;
        }
        ~TwoDConcurrentQueue() {
            delete[] queues;
        }
    };

    class Stacks {
    protected:
        struct StackInfo {
            Item *stack;
            int len;
        };

        Item *data;
        int *index;
        StackInfo *stackInfo;

        const int * const label;

    public:
        Stacks(int n, int threadNum, const int *nodeNumberCount, const int *label) :
                label(label) {
            size_t dataLen = sizeof(Item) * n, indexLen = sizeof(int) * n, stackInfoLen = sizeof(StackInfo) * threadNum;
            char *buffer = (char*) malloc(dataLen + indexLen + stackInfoLen);
            assert(buffer!=NULL);
            data = (Item*) (buffer);
            index = (int*) (buffer + dataLen);
            stackInfo = (StackInfo*) (buffer + dataLen + indexLen);

            memset(index, 0xff, sizeof(int) * n);

            assert(threadNum > 0);
            stackInfo[0].stack = data;
            stackInfo[0].len = 0;
            for (int i = 1; i < threadNum; ++i) {
                stackInfo[i].stack = stackInfo[i - 1].stack + nodeNumberCount[i - 1];
                stackInfo[i].len = 0;
            }
        }

        virtual ~Stacks() {
            free(data);
        }

        inline void addOrUpdate(int threadNo, int key, int value) {
            Item *&data = stackInfo[threadNo].stack;
            int &len = stackInfo[threadNo].len;
            const int &lkey = label[key];
            if (index[lkey] != -1) {
                if (data[index[lkey]].value > value) data[index[lkey]].value = value;
            } else {
                data[len].key = key;
                data[len].value = value;
                index[lkey] = len;
                ++len;
            }
        }

        inline bool empty(int threadNo) const {
            return stackInfo[threadNo].len == 0;
        }

        inline void removeTop(int threadNo, Item *pTop) {
            Item *&data = stackInfo[threadNo].stack;
            int &len = stackInfo[threadNo].len;
            assert(len > 0);
            --len;
            index[label[data[len].key]] = -1;
            if (pTop != NULL) *pTop = data[len];
        }

        inline const Item& top(int threadNo) const {
            Item *&data = stackInfo[threadNo].stack;
            int &len = stackInfo[threadNo].len;
            assert(len > 0);
            return data[len - 1];
        }
    };

    class OMPLockStacks: public Stacks {
        // lock here
        omp_lock_t *locks;
        int threadNum;
    public:
        OMPLockStacks(int n, int threadNum, const int *nodeNumberCount, const int *label) :
                Stacks(n, threadNum, nodeNumberCount, label), threadNum(threadNum) {
            locks = (omp_lock_t*) malloc(sizeof(omp_lock_t) * threadNum);
            assert(locks != NULL);
            for (int i = 0; i < threadNum; ++i)
                omp_init_lock(locks + i);
        }

        ~OMPLockStacks() {
            for (int i = 0; i < threadNum; ++i)
                omp_destroy_lock(locks + i);
            free(locks);
        }

        inline void addOrUpdateSafe(int threadNo, int key, int value) {
            omp_set_lock(locks + threadNo);
            addOrUpdate(threadNo, key, value);
            omp_unset_lock(locks + threadNo);
        }

        inline void removeTopSafe(int threadNo, Item *pTop) {
            omp_set_lock(locks + threadNo);
            removeTop(threadNo, pTop);
            omp_unset_lock(locks + threadNo);
        }
    };

    class Heaps {
        struct HeapInfo {
            Item *heap;
            int len;
        };

        Item *data;
        int *index;
        HeapInfo *heapInfo;

    public:
        Heaps(int n, int threadNum, const int *nodeNumberCount) {
            size_t dataLen = sizeof(Item) * n, indexLen = sizeof(int) * n, heapInfoLen = sizeof(HeapInfo) * threadNum;
            char *buffer = (char*) malloc(dataLen + indexLen + heapInfoLen);
            assert(buffer!=NULL);
            data = (Item*) (buffer);
            index = (int*) (buffer + dataLen);
            heapInfo = (HeapInfo*) (buffer + dataLen + indexLen);

            memset(index, 0xff, sizeof(int) * n);

            heapInfo[0].heap = data;
            heapInfo[0].len = 0;
            for (int i = 1; i < threadNum; ++i) {
                heapInfo[i].heap = heapInfo[i - 1].heap + nodeNumberCount[i - 1];
                heapInfo[i].len = 0;
            }
        }

        ~Heaps() {
            free(data);
        }

    private:

        inline int fa(int x) {
            return (x - 1) / 2;
        }

        inline void up(HeapInfo &heapInfo, int x) {
            Item *&data = heapInfo.heap;
            while (x > 0) {
                int tmp = (x - 1) / 2;
                if (data[tmp].value > data[x].value) {
                    swap(data[tmp], data[x]);
                    index[data[tmp].key] = tmp;
                    index[data[x].key] = x;
                    x = tmp;
                } else
                    break;
            }
        }

        inline void down(HeapInfo &heapInfo, int x) {
            Item *&data = heapInfo.heap;
            int &len = heapInfo.len;
            while (x * 2 + 1 < len) {
                int tmp = (x * 2 + 2 >= len || data[x * 2 + 1].value < data[x * 2 + 2].value) ? x * 2 + 1 : x * 2 + 2;
                if (data[tmp].value < data[x].value) {
                    swap(data[tmp], data[x]);
                    index[data[tmp].key] = tmp;
                    index[data[x].key] = x;
                    x = tmp;
                } else
                    break;
            }
        }

    public:

        inline void add(int threadNo, int key, int value) {
            Item *&data = heapInfo[threadNo].heap;
            int &len = heapInfo[threadNo].len;

            data[len].key = key;
            data[len].value = value;
            index[key] = len;
            ++len;
            up(heapInfo[threadNo], len - 1);
        }

        inline void update(int threadNo, int key, int value) {
            Item *&data = heapInfo[threadNo].heap;

            data[index[key]].value = value;
            up(heapInfo[threadNo], index[key]);
        }

        inline void addOrUpdate(int threadNo, int key, int value) {
            Item *&data = heapInfo[threadNo].heap;
            if (index[key] == -1)
                add(threadNo, key, value);
            else if (value < data[index[key]].value) update(threadNo, key, value);
        }

        inline void removeTop(int threadNo, Item *pData) {
            Item *&data = heapInfo[threadNo].heap;
            int &len = heapInfo[threadNo].len;

            --len;
            swap(data[0], data[len]);
            if (pData != NULL) *pData = data[len];
            index[data[0].key] = 0;
            index[data[len].key] = -1;
            down(heapInfo[threadNo], 0);
        }

        inline const Item& top(int threadNo) const {
            return heapInfo[threadNo].heap[0];
        }

        inline bool empty(int threadNo) const {
            return heapInfo[threadNo].len == 0;
        }

        inline void print(int threadNo) const {
            Item *&data = heapInfo[threadNo].heap;
            for (int i = 0; i < heapInfo[threadNo].len; ++i)
                printf("(%d, %d) ", data[i].key, data[i].value);
            printf("\n");
        }

    };

}

#endif
